#include <OsgClassDataSource/MatrixTransformOsg.h>
#include <iostream>
#include <SceneViewerFunctions.h>
#include <FunctionsPluginOsg.h>

MatrixTransformOsg::MatrixTransformOsg( QObject *parent /*= 0*/ ) : TransformOsg(parent)
{
    m_object = new osg::MatrixTransform;
    osg::MatrixTransform* matrix_node = dynamic_cast<osg::MatrixTransform*>(m_object.get());
    if(matrix_node == NULL) return;
    osg::Matrix m = matrix_node->getMatrix();
    osg::Vec3d trans = m.getTrans();
    m_matrix(0,3) = trans.x();
    m_matrix(1,3) = trans.y();
    m_matrix(2,3) = trans.z();

    osg::Vec3d rotate = SceneViewerFunctions::osg_matrix_getRotate(m);
    m_matrix(0,2) = rotate.x();
    m_matrix(1,2) = rotate.y();
    m_matrix(2,2) = rotate.z();

    osg::Vec3d scale = m.getScale();
    m_matrix(0,1) = scale.x();
    m_matrix(1,1) = scale.y();
    m_matrix(2,1) = scale.z();

    this->refreshInfoContainer();

    addRecoverPropertyName("Matrix");
}

MatrixTransformOsg::~MatrixTransformOsg()
{

}

QMatrix4x4 MatrixTransformOsg::getMatrix()
{
    //std::cout<<"MatrixTransformOsg::getMatrix"<<std::endl;
    /**/
    osg::MatrixTransform* matrix_node = dynamic_cast<osg::MatrixTransform*>(m_object.get());
    osg::Matrix m = matrix_node->getMatrix();
    osg::Vec3d trans = m.getTrans();
    m_matrix(0,3) = trans.x();
    m_matrix(1,3) = trans.y();
    m_matrix(2,3) = trans.z();

    osg::Vec3d rotate = SceneViewerFunctions::osg_matrix_getRotate(m);
    m_matrix(0,2) = rotate.x();
    m_matrix(1,2) = rotate.y();
    m_matrix(2,2) = rotate.z();

    osg::Vec3d scale = m.getScale();
    m_matrix(0,1) = scale.x();
    m_matrix(1,1) = scale.y();
    m_matrix(2,1) = scale.z();

    return m_matrix;
}

void MatrixTransformOsg::setMatrix(QMatrix4x4 matrix)
{
    //std::cout<<"MatrixTransformOsg::setMatrix"<<std::endl;
/**/
    if(m_matrix == matrix)
    {
        //return;
    }
    FunctionsPluginOsg::EDesignerCoreSimulationWaitUpdateEndAndStopSim();

    //std::cout<<"set matrix value"<<std::endl;
    osg::MatrixTransform* matrix_node = dynamic_cast<osg::MatrixTransform*>(m_object.get());
    if(matrix_node == NULL) return;

    //osg::Matrix m = SceneViewerFunctions::osg_matrix_RotateTrans(osg::Vec3(matrix(0,2),matrix(1,2), matrix(2,2)),osg::Vec3(matrix(0,3),matrix(1,3), matrix(2,3)));
    osg::Matrix m = SceneViewerFunctions::osg_matrix_ScaleRotateTrans(osg::Vec3(matrix(0,1),matrix(1,1), matrix(2,1)),
                                                                      osg::Vec3(matrix(0,2),matrix(1,2), matrix(2,2)),
                                                                      osg::Vec3(matrix(0,3),matrix(1,3), matrix(2,3)));

    //std::cout<<"MatrixTransformOsg::setMatrix scale="<<matrix(0,1)<<","<<matrix(1,1)<<","<<matrix(2,1)<<std::endl;

    matrix_node->setMatrix(m);

    m_matrix = matrix;
    emit signalMatrixChanged(m_matrix);
//    FunctionsPluginOsg::EDesignerCoreSimulationStopWaitUpdateEnd();


}

QVarintTrigger MatrixTransformOsg::getTriggerEliminateScale()
{
    return QVarintTrigger();
}

void MatrixTransformOsg::setTriggerEliminateScale(QVarintTrigger trigger)
{
    osg::MatrixTransform* matrix_node = dynamic_cast<osg::MatrixTransform*>(m_object.get());
    if(matrix_node != NULL)
    {
        SceneViewerFunctions::matrixTransformEliminateScale(matrix_node);
    }

}

QVarintTrigger MatrixTransformOsg::getTriggerEliminateRotate()
{
    return QVarintTrigger();
}

void MatrixTransformOsg::setTriggerEliminateRotate(QVarintTrigger trigger)
{
    osg::MatrixTransform* matrix_node = dynamic_cast<osg::MatrixTransform*>(m_object.get());
    if(matrix_node != NULL)
    {
        SceneViewerFunctions::matrixTransformEliminateRotate(matrix_node);
    }
}

QVarintTrigger MatrixTransformOsg::getTriggerSmall_100()
{
    return QVarintTrigger();
}

void MatrixTransformOsg::setTriggerSmall_100(QVarintTrigger trigger)
{
    this->setTriggerEliminateRotate(trigger);
    QMatrix4x4 currentQMaxtrix = this->getMatrix();

    currentQMaxtrix(0,3) *= 0.01;
    currentQMaxtrix(1,3) *= 0.01;
    currentQMaxtrix(2,3) *= 0.01;

    currentQMaxtrix(0,1) = 0.01;
    currentQMaxtrix(1,1) = 0.01;
    currentQMaxtrix(2,1) = 0.01;

    this->setMatrix(currentQMaxtrix);
    this->setTriggerEliminateScale(trigger);
}

QVarintTrigger MatrixTransformOsg::getTriggerOBBboxPrint()
{
    return QVarintTrigger();
}

void MatrixTransformOsg::setTriggerOBBboxPrint(QVarintTrigger trigger)
{
    osg::MatrixTransform* matrix_node = dynamic_cast<osg::MatrixTransform*>(m_object.get());
    if(matrix_node == NULL) return;

    osg::ref_ptr<osg::Vec3Array> obbVecArray = SceneViewerFunctions::computeObbIgnoreSubSubChildNode(matrix_node);
    std::cout<<"MatrixTransformOsg::setTriggerOBBboxPrint size="<<obbVecArray->size()<<std::endl;
    std::cout<<obbVecArray->at(0).x()<<","<<obbVecArray->at(0).y()<<","<<obbVecArray->at(0).z()<<std::endl;
    std::cout<<obbVecArray->at(1).x()<<","<<obbVecArray->at(1).y()<<","<<obbVecArray->at(1).z()<<std::endl;
    std::cout<<obbVecArray->at(2).x()<<","<<obbVecArray->at(2).y()<<","<<obbVecArray->at(2).z()<<std::endl;
    std::cout<<obbVecArray->at(3).x()<<","<<obbVecArray->at(3).y()<<","<<obbVecArray->at(3).z()<<std::endl;
    std::cout<<obbVecArray->at(4).x()<<","<<obbVecArray->at(4).y()<<","<<obbVecArray->at(4).z()<<std::endl;
    std::cout<<obbVecArray->at(5).x()<<","<<obbVecArray->at(5).y()<<","<<obbVecArray->at(5).z()<<std::endl;
    std::cout<<obbVecArray->at(6).x()<<","<<obbVecArray->at(6).y()<<","<<obbVecArray->at(6).z()<<std::endl;
    std::cout<<obbVecArray->at(6).x()<<","<<obbVecArray->at(6).y()<<","<<obbVecArray->at(6).z()<<std::endl;

}

void MatrixTransformOsg::copyFrom(EObject *objPtr)
{
    TransformOsg::copyFrom(objPtr);
    MatrixTransformOsg* objOsgPtr = dynamic_cast<MatrixTransformOsg*>(objPtr);
    if(objOsgPtr == NULL)return;
    this->m_matrix = objOsgPtr->m_matrix;
}

EObject *MatrixTransformOsg::clone()
{
    MatrixTransformOsg* newObj = new MatrixTransformOsg();
    newObj->copyFrom(this);
    return newObj;
}


